package hdgeom.procedures;

import hdgeom.primitives.AbstractPolygon;

import java.awt.geom.Line2D;
import java.util.Collections;
import java.util.Vector;

import processing.core.PVector;
import simpleGeom.HasPos2D;
import simpleGeom.Polygon2D;
import simpleGeom.Vec2D;





	public class POrientedBoundingBox extends AbstractPolygon{

		PCaliper[] calipers;

		double[] angles;

		public HasPos2D pMinX;

		public HasPos2D pMinY;

		public HasPos2D pMaxX;

		public HasPos2D pMaxY;

		public Vector<HasPos2D> convexHull;

		// int[] indexPoints;
		int[] indexPoints = new int[4];

		double[] deltaNextDist = new double[4];

		double[] angle = new double[4];

		double[] deltaAngle = new double[4];

		double rotAngle;

		HasPos2D[] rect;

		double rectAngle;

		HasPos2D rectOrigin;

		double minWidth;

		double minHeight;

		double minArea;

		public POrientedBoundingBox(Vector<? extends HasPos2D> convexHull) {
			calculateBoundingBoxFromConvexhull(convexHull);
		}

		public void calculateBoundingBoxFromConvexhull(Vector<? extends HasPos2D> convexHull) {
			this.convexHull = (Vector<HasPos2D>) convexHull;
			HasPos2D p0;
			HasPos2D p2 = convexHull.get(0);
			HasPos2D p1 = convexHull.lastElement();
			double area = Polygon2D.getArea(convexHull);
			if (area > 0) {
				System.out.println("area");
			}
			for (int i = 0; i < this.convexHull.size(); i++) {
				p0 = p1;
				p1 = p2;
				p2 = this.convexHull.get((i + 1) % convexHull.size());
				Vec2D p = Vec2D.sub(p2, p1);
				double d = p.mag();
				if (d < 0.0001) {
					this.convexHull.remove(i);
				} else {
					double cAngle = Vec2D.getAngleBetweenCCW(p0, p1, p2);
					double dA = Math.abs(cAngle - Math.PI);
					if (dA < 0.01) {
						// System.out.println("same: " + dA);
						this.convexHull.remove(i);
					}

				}
			}
			// Collections.reverse(convexHull);
			this.calculateOBB();
		}

		void calculateOBB() {

			rect = new HasPos2D[4];
			int minX = 0;
			int maxX = 0;
			int minY = 0;
			int maxY = 0;

			angles = new double[convexHull.size()];

			HasPos2D p2 = convexHull.get(0);
			HasPos2D p1;

			pMinX = p2;
			pMinY = p2;
			pMaxX = p2;
			pMaxY = p2;

			for (int i = 0; i < convexHull.size(); i++) {
				p1 = p2;
				p2 = convexHull.get((i + 1) % convexHull.size());
				Vec2D p = Vec2D.sub(p2, p1);
				angles[i] = Vec2D.getAngle(p);

				if (pMinX.getX() > p1.getX()) {
					pMinX = p1;
					minX = i;
				} else if (pMaxX.getX() < p1.getX()) {
					pMaxX = p1;
					maxX = i;
				}
				if (pMinY.getY() > p1.getY()) {
					pMinY = p1;
					minY = i;
				} else if (pMaxY.getY() < p1.getY()) {
					pMaxY = p1;
					maxY = i;
				}
			}
			minWidth = pMaxX.getX() - pMinX.getX() + 1;
			minHeight = pMaxY.getY() - pMinY.getY() + 1;
			minArea = minWidth * minHeight;

			calipers = new PCaliper[4];
			calipers[0] = new PCaliper(convexHull, angles, minY, 0);
			calipers[1] = new PCaliper(convexHull, angles, maxX, Math.PI * 0.5);
			calipers[2] = new PCaliper(convexHull, angles, maxY, Math.PI);
			calipers[3] = new PCaliper(convexHull, angles, minX, Math.PI * 1.5);

			/*
			 * calipers[0] = new Caliper(convexHull, angles, minY, Math.PI);
			 * calipers[1] = new Caliper(convexHull, angles, maxX, Math.PI * 0.5);
			 * calipers[2] = new Caliper(convexHull, angles, maxY, 0); calipers[3] =
			 * new Caliper(convexHull, angles, minX, Math.PI * 1.5);
			 */
			analyse();
			rotate();
		}

		void analyse() {
			double cWidth = calipers[0].getDistance(calipers[2].getPoint());
			double cHeight = calipers[1].getDistance(calipers[3].getPoint());
			double cArea = cWidth * cHeight;

			if (minArea > cArea) {
				minWidth = cWidth;
				minHeight = cWidth;
				minArea = cArea;
				rectAngle = calipers[0].angle;
				makeRect();
			}
		}

		void rotate() {
			int r = 0;
			while (calipers[0].angle > Math.PI * -1.5) {

				rotAngle = 1000;
				for (int i = 0; i < 4; i++) {
					PCaliper c = calipers[i];
					// System.out.println("delta"+c.deltaAngle);
					if (c.deltaAngle < rotAngle) {
						rotAngle = c.deltaAngle;
					}
				}

				for (int i = 0; i < 4; i++) {
					PCaliper c = calipers[i];
					c.rotate(rotAngle);
				}
				// System.out.println("calipers[0].angle: "+calipers[0].angle);
				analyse();
				r++;
			}

		}

		void makeRect() {
			rect[0] = Vec2D.lineLineIntersection(calipers[0].getLine2D(1),
					calipers[3].getLine2D(1));
			rect[1] = Vec2D.lineLineIntersection(calipers[1].getLine2D(1),
					calipers[0].getLine2D(1));
			rect[2] = Vec2D.lineLineIntersection(calipers[2].getLine2D(1),
					calipers[1].getLine2D(1));
			rect[3] = Vec2D.lineLineIntersection(calipers[3].getLine2D(1),
					calipers[2].getLine2D(1));
		}

		

		public HasPos2D[] getOrientedBoundingBox() {
			return rect;
		}
		public Vector<HasPos2D> getOrientedPoly() {
			Vector<HasPos2D>b=new Vector<HasPos2D>();
			for (int i=0;i<rect.length;i++){
				b.add(rect[i]);
			}
			return b;
		}

		@Override
		public PVector getVertex(int i) {
			// TODO Auto-generated method stub
			return new PVector(rect[i].getX(),rect[i].getY());
		}

		@Override
		public float getX(int i) {
			// TODO Auto-generated method stub
			return rect[i].getX();
		}

		@Override
		public float getY(int i) {
			// TODO Auto-generated method stub
			return rect[i].getY();
		}

		@Override
		public void setPosition(int i, float x, float y) {
			// TODO Auto-generated method stub
			rect[i].setX(x);
			rect[i].setY(y);
		}

		@Override
		public int getNumVertices() {
			// TODO Auto-generated method stub
			return 4;
		}

		@Override
		public void getPosition(int i, float[] coords) {
			// TODO Auto-generated method stub
			HasPos2D hp=rect[i];
			coords[0]=hp.getX();
			coords[1]=hp.getY();
		}

		@Override
		public void setPosition(int i, float x, float y, float z) {
			// TODO Auto-generated method stub
			rect[i].setX(x);
			rect[i].setY(y);
		}

	}

	 class PCaliper {
		public Vector<HasPos2D> convexHull;

		public double[] angles;

		public PCaliper(Vector<HasPos2D> convexHull, double[] angles, int index,
				double angle) {
			this.angles = angles;
			this.convexHull = convexHull;
			this.angle = angle;
			this.point = index;

			deltaAngle = Vec2D.getAngleBetweenCCW(angle, angles[point]);

			if (deltaAngle <= 0) {
				point = (point + 1) % convexHull.size();
				double nextDeltaAngle = Vec2D.getAngleBetweenCCW(angles[point],
						angle);
				deltaAngle = nextDeltaAngle;
			}
		}

		public PCaliper(int index, double angle) {
			this.angle = angle;
			this.point = index;
		}

		double angle;

		double deltaAngle;

		int point;

		HasPos2D[] getLine2D(double l) {
			HasPos2D[] pts = new HasPos2D[2];
			pts[0] = convexHull.get(point);
			pts[1] = Vec2D.getPolar(pts[0].getX(),pts[0].getY(),(float) angle,(float)  l);
			return pts;
		}

		public double getDistance(HasPos2D p) {
			HasPos2D[] pts = getLine2D(1);
			HasPos2D p1 = pts[0];
			HasPos2D p2 = pts[1];
			return Line2D.ptLineDist(p1.getX(), p1.getY(), p2.getX(), p2.getY(), p.getX(), p.getY());
			// return getLine2D(1).ptLineDist(p);
		}

		public HasPos2D getPoint() {
			return convexHull.get(point);
		}

		void rotate(double rotAngle) {

			angle -= rotAngle;
			angle = angle % (Math.PI * 2.);

			deltaAngle -= rotAngle;

			if (deltaAngle <= 0) {

				point = (point + 1) % convexHull.size();
				double nextDeltaAngle = Vec2D.getAngleBetweenCCW(angle,
						angles[point]);

				deltaAngle = nextDeltaAngle;
			}

		}
	 }

	  
	

